//
// File: EKF_v5g.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 18-Nov-2024 11:14:11
//

// Include Files
#include "EKF_v5g.h"
#include "xgeqp3.h"
#include <algorithm>
#include <cmath>
#include <cstring>

// Variable Definitions
static double P0[1024];

// Function Definitions
//
// Arguments    : const double dg_a[32]
//                const double do_a[32]
//                const double sig_a[32]
//                const double sio_a[32]
//                double dgo
//                double doo
//                double sigo
//                double sioo
//                const double aIN[32]
//                double dg
//                double b_do
//                double sig
//                double sio
//                double aOUT[32]
// Return Type  : void
//
void EKF_v5g(const double dg_a[32], const double do_a[32],
             const double sig_a[32], const double sio_a[32], double dgo,
             double doo, double sigo, double sioo, const double aIN[32],
             double dg, double b_do, double sig, double sio, double aOUT[32])
{
  static const double Q[1024]{
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
      0.0,  0.0, 0.0, 0.01};
  static const double y[16]{0.1, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
                            0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.1};
  double b_y[1024];
  double c_I[1024];
  double K_k_data[128];
  double O_k_data[128];
  double y_data[128];
  double b_dg_a[64];
  double A_data[16];
  double R_data[16];
  double o_k_data[4];
  double odk_data[4];
  double bkj;
  int jpvt_data[4];
  int A_size[2];
  int K_k_size_idx_1;
  int O_k_size_idx_0;
  int R_size_idx_1;
  int aoffset;
  int boffset;
  int coffset;
  int i;
  int i1;
  int minmn;
  int rankA;
  signed char b_I[1024];
  // 初始化静态变量
  if (doo < 2.0) {
    O_k_size_idx_0 = 4;
    for (i = 0; i < 32; i++) {
      O_k_data[4 * i] = dg_a[i];
      O_k_data[4 * i + 1] = do_a[i];
      O_k_data[4 * i + 2] = sig_a[i];
      O_k_data[4 * i + 3] = sio_a[i];
    }
    odk_data[0] = dg;
    odk_data[1] = b_do;
    odk_data[2] = sig;
    odk_data[3] = sio;
    rankA = 4;
    R_size_idx_1 = 4;
    std::copy(&y[0], &y[16], &R_data[0]);
    minmn = 4;
    o_k_data[0] = dgo;
    o_k_data[1] = doo;
    o_k_data[2] = sigo;
    o_k_data[3] = sioo;
  } else {
    for (i = 0; i < 32; i++) {
      minmn = i << 1;
      b_dg_a[minmn] = dg_a[i];
      b_dg_a[minmn + 1] = sig_a[i];
    }
    O_k_size_idx_0 = 2;
    std::copy(&b_dg_a[0], &b_dg_a[64], &O_k_data[0]);
    odk_data[0] = dg;
    odk_data[1] = sig;
    rankA = 2;
    R_size_idx_1 = 2;
    R_data[0] = 0.1;
    R_data[1] = 0.0;
    R_data[2] = 0.0;
    R_data[3] = 0.1;
    minmn = 2;
    o_k_data[0] = dgo;
    o_k_data[1] = sigo;
  }
  //      O_k=[dg_a;do_a;sig_a;sio_a];
  //      odk=[dg;do;sig;sio];
  //      R=0.1*eye(4);
  //      o_k=[dgo;doo;sigo;sioo];
  for (i = 0; i < minmn; i++) {
    o_k_data[i] -= odk_data[i];
  }
  for (int j{0}; j < 32; j++) {
    coffset = j * O_k_size_idx_0;
    boffset = j << 5;
    std::memset(&y_data[coffset], 0, O_k_size_idx_0 * sizeof(double));
    for (int k{0}; k < 32; k++) {
      aoffset = k * O_k_size_idx_0;
      bkj = P0[boffset + k];
      for (int b_i{0}; b_i < O_k_size_idx_0; b_i++) {
        i = coffset + b_i;
        y_data[i] += O_k_data[aoffset + b_i] * bkj;
      }
    }
  }
  for (int j{0}; j < O_k_size_idx_0; j++) {
    coffset = j * O_k_size_idx_0;
    std::memset(&A_data[coffset], 0, O_k_size_idx_0 * sizeof(double));
    for (int k{0}; k < 32; k++) {
      aoffset = k * O_k_size_idx_0;
      bkj = O_k_data[k * O_k_size_idx_0 + j];
      for (int b_i{0}; b_i < O_k_size_idx_0; b_i++) {
        i = coffset + b_i;
        A_data[i] += y_data[aoffset + b_i] * bkj;
      }
    }
  }
  K_k_size_idx_1 = O_k_size_idx_0;
  for (int j{0}; j < O_k_size_idx_0; j++) {
    coffset = j << 5;
    std::memset(&K_k_data[coffset], 0, 32U * sizeof(double));
    for (int k{0}; k < 32; k++) {
      aoffset = k << 5;
      bkj = O_k_data[k * O_k_size_idx_0 + j];
      for (int b_i{0}; b_i < 32; b_i++) {
        i = coffset + b_i;
        K_k_data[i] += P0[aoffset + b_i] * bkj;
      }
    }
  }
  minmn = rankA * R_size_idx_1;
  for (i = 0; i < minmn; i++) {
    R_data[i] += A_data[i];
  }
  if (rankA == R_size_idx_1) {
    std::copy(&R_data[0], &R_data[minmn], &A_data[0]);
    jpvt_data[0] = 1;
    minmn = 1;
    for (int k{2}; k <= R_size_idx_1; k++) {
      minmn++;
      jpvt_data[k - 1] = minmn;
    }
    if (R_size_idx_1 - 1 <= R_size_idx_1) {
      i = R_size_idx_1;
    } else {
      i = 3;
    }
    for (int j{0}; j <= i - 2; j++) {
      int jj;
      int y_size_idx_0;
      y_size_idx_0 = R_size_idx_1 - j;
      boffset = j * (R_size_idx_1 + 1);
      jj = j * (R_size_idx_1 + 1);
      aoffset = boffset + 2;
      if (y_size_idx_0 < 1) {
        minmn = -1;
      } else {
        minmn = 0;
        if (y_size_idx_0 > 1) {
          bkj = std::abs(A_data[jj]);
          for (int k{2}; k <= y_size_idx_0; k++) {
            double wj;
            wj = std::abs(A_data[(boffset + k) - 1]);
            if (wj > bkj) {
              minmn = k - 1;
              bkj = wj;
            }
          }
        }
      }
      if (A_data[jj + minmn] != 0.0) {
        if (minmn != 0) {
          minmn += j;
          jpvt_data[j] = minmn + 1;
          for (int k{0}; k < R_size_idx_1; k++) {
            rankA = k * R_size_idx_1;
            coffset = j + rankA;
            bkj = A_data[coffset];
            i1 = minmn + rankA;
            A_data[coffset] = A_data[i1];
            A_data[i1] = bkj;
          }
        }
        i1 = jj + y_size_idx_0;
        for (int b_i{aoffset}; b_i <= i1; b_i++) {
          A_data[b_i - 1] /= A_data[jj];
        }
      }
      coffset = boffset + R_size_idx_1;
      boffset = jj + R_size_idx_1;
      for (aoffset = 0; aoffset <= y_size_idx_0 - 2; aoffset++) {
        minmn = coffset + aoffset * R_size_idx_1;
        bkj = A_data[minmn];
        if (A_data[minmn] != 0.0) {
          i1 = boffset + 2;
          rankA = y_size_idx_0 + boffset;
          for (minmn = i1; minmn <= rankA; minmn++) {
            A_data[minmn - 1] += A_data[((jj + minmn) - boffset) - 1] * -bkj;
          }
        }
        boffset += R_size_idx_1;
      }
    }
    for (int j{0}; j < R_size_idx_1; j++) {
      minmn = (j << 5) - 1;
      coffset = R_size_idx_1 * j;
      for (int k{0}; k < j; k++) {
        boffset = k << 5;
        i = k + coffset;
        if (A_data[i] != 0.0) {
          for (int b_i{0}; b_i < 32; b_i++) {
            i1 = (b_i + minmn) + 1;
            K_k_data[i1] -= A_data[i] * K_k_data[b_i + boffset];
          }
        }
      }
      bkj = 1.0 / A_data[j + coffset];
      for (int b_i{0}; b_i < 32; b_i++) {
        i = (b_i + minmn) + 1;
        K_k_data[i] *= bkj;
      }
    }
    for (int j{R_size_idx_1}; j >= 1; j--) {
      minmn = ((j - 1) << 5) - 1;
      coffset = R_size_idx_1 * (j - 1) - 1;
      i = j + 1;
      for (int k{i}; k <= R_size_idx_1; k++) {
        boffset = (k - 1) << 5;
        i1 = k + coffset;
        if (A_data[i1] != 0.0) {
          for (int b_i{0}; b_i < 32; b_i++) {
            rankA = (b_i + minmn) + 1;
            K_k_data[rankA] -= A_data[i1] * K_k_data[b_i + boffset];
          }
        }
      }
    }
    i = R_size_idx_1 - 1;
    for (int j{i}; j >= 1; j--) {
      i1 = jpvt_data[j - 1];
      if (i1 != j) {
        for (int b_i{0}; b_i < 32; b_i++) {
          rankA = b_i + 32 * (j - 1);
          bkj = K_k_data[rankA];
          minmn = b_i + 32 * (i1 - 1);
          K_k_data[rankA] = K_k_data[minmn];
          K_k_data[minmn] = bkj;
        }
      }
    }
  } else {
    double B_data[128];
    int y_size_idx_0;
    for (i = 0; i < 32; i++) {
      for (i1 = 0; i1 < O_k_size_idx_0; i1++) {
        B_data[i1 + O_k_size_idx_0 * i] = K_k_data[i + 32 * i1];
      }
    }
    A_size[0] = R_size_idx_1;
    A_size[1] = rankA;
    for (i = 0; i < rankA; i++) {
      for (i1 = 0; i1 < R_size_idx_1; i1++) {
        A_data[i1 + R_size_idx_1 * i] = R_data[i + rankA * i1];
      }
    }
    int jpvt_size[2];
    coder::internal::lapack::xgeqp3(A_data, A_size, odk_data, &minmn, jpvt_data,
                                    jpvt_size);
    rankA = 0;
    if (A_size[0] < A_size[1]) {
      minmn = A_size[0];
      coffset = A_size[1];
    } else {
      minmn = A_size[1];
      coffset = A_size[0];
    }
    if (minmn > 0) {
      bkj = 2.2204460492503131E-15 * static_cast<double>(coffset) *
            std::abs(A_data[0]);
      while ((rankA < minmn) &&
             (std::abs(A_data[rankA + A_size[0] * rankA]) > bkj)) {
        rankA++;
      }
    }
    y_size_idx_0 = A_size[1];
    minmn = A_size[1] << 5;
    if (minmn - 1 >= 0) {
      std::memset(&y_data[0], 0, minmn * sizeof(double));
    }
    boffset = A_size[0];
    minmn = A_size[0];
    aoffset = A_size[1];
    if (minmn <= aoffset) {
      aoffset = minmn;
    }
    for (int j{0}; j < aoffset; j++) {
      if (odk_data[j] != 0.0) {
        i = j + 2;
        for (int k{0}; k < 32; k++) {
          double wj;
          coffset = O_k_size_idx_0 * k;
          minmn = j + coffset;
          bkj = B_data[minmn];
          wj = bkj;
          for (int b_i{i}; b_i <= boffset; b_i++) {
            wj +=
                A_data[(b_i + A_size[0] * j) - 1] * B_data[(b_i + coffset) - 1];
          }
          wj *= odk_data[j];
          if (wj != 0.0) {
            B_data[minmn] = bkj - wj;
            for (int b_i{i}; b_i <= boffset; b_i++) {
              minmn = (b_i + coffset) - 1;
              B_data[minmn] -= A_data[(b_i + A_size[0] * j) - 1] * wj;
            }
          }
        }
      }
    }
    for (int k{0}; k < 32; k++) {
      for (int b_i{0}; b_i < rankA; b_i++) {
        y_data[(jpvt_data[b_i] + y_size_idx_0 * k) - 1] =
            B_data[b_i + O_k_size_idx_0 * k];
      }
      for (int j{rankA}; j >= 1; j--) {
        minmn = y_size_idx_0 * k;
        coffset = (jpvt_data[j - 1] + minmn) - 1;
        boffset = A_size[0] * (j - 1);
        y_data[coffset] /= A_data[(j + boffset) - 1];
        for (int b_i{0}; b_i <= j - 2; b_i++) {
          coffset = (jpvt_data[b_i] + minmn) - 1;
          y_data[coffset] -= y_data[(jpvt_data[j - 1] + y_size_idx_0 * k) - 1] *
                             A_data[b_i + boffset];
        }
      }
    }
    K_k_size_idx_1 = A_size[1];
    for (i = 0; i < y_size_idx_0; i++) {
      for (i1 = 0; i1 < 32; i1++) {
        K_k_data[i1 + 32 * i] = y_data[i + y_size_idx_0 * i1];
      }
    }
  }
  std::memset(&aOUT[0], 0, 32U * sizeof(double));
  for (int k{0}; k < K_k_size_idx_1; k++) {
    aoffset = k << 5;
    for (int b_i{0}; b_i < 32; b_i++) {
      aOUT[b_i] += K_k_data[aoffset + b_i] * o_k_data[k];
    }
  }
  for (int j{0}; j < 32; j++) {
    coffset = j << 5;
    boffset = j * O_k_size_idx_0;
    std::memset(&b_y[coffset], 0, 32U * sizeof(double));
    for (int k{0}; k < K_k_size_idx_1; k++) {
      aoffset = k << 5;
      bkj = O_k_data[boffset + k];
      for (int b_i{0}; b_i < 32; b_i++) {
        minmn = coffset + b_i;
        b_y[minmn] += K_k_data[aoffset + b_i] * bkj;
      }
    }
  }
  std::memset(&b_I[0], 0, 1024U * sizeof(signed char));
  for (int k{0}; k < 32; k++) {
    b_I[k + (k << 5)] = 1;
  }
  for (i = 0; i < 1024; i++) {
    b_y[i] = static_cast<double>(b_I[i]) - b_y[i];
  }
  for (i = 0; i < 32; i++) {
    for (i1 = 0; i1 < 32; i1++) {
      bkj = 0.0;
      for (rankA = 0; rankA < 32; rankA++) {
        bkj += b_y[i + (rankA << 5)] * P0[rankA + (i1 << 5)];
      }
      minmn = i + (i1 << 5);
      c_I[minmn] = bkj + Q[minmn];
    }
  }
  std::copy(&c_I[0], &c_I[1024], &P0[0]);
  for (i = 0; i < 32; i++) {
    aOUT[i] = aIN[i] - aOUT[i];
  }
  //      aOUT=[aUPDATE(1:16);aUPDATE(17:32)];
}

//
// Arguments    : void
// Return Type  : void
//
void EKF_v5g_init()
{
  static const signed char iv[1024]{
      1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
  for (int i{0}; i < 1024; i++) {
    P0[i] = iv[i];
  }
}

//
// File trailer for EKF_v5g.cpp
//
// [EOF]
//
